#include <GL/gl.h>
#include "windpoint.h"
#include "glutproject.h"
#include "bmath.h"
#include <sstream>

using namespace std;

//-----------------------------------------------------------------------------
// WheelChair Representation Class 
//-----------------------------------------------------------------------------
class WheelChair : public WindPoint
{
    public:
        WheelChair(const Point& pos) : 
            WindPoint(pos),
            m_LeftWheelSpeed(0),
            m_RightWheelSpeed(0),
            m_BreakValue(0)
        {}
        void Draw()
        {
            glPushMatrix();
            glPointSize(10.0);
            glLineWidth(2.0);

            Point arrow(50, GetSpeed().GetAngle());
            arrow.SetXY(arrow.GetX() + GetPosition().GetX(), arrow.GetY() + GetPosition().GetY());

            glColor3f(0, 0, 0);
            glBegin(GL_POINTS);
            glVertex2d(GetPosition().GetX(), GetPosition().GetY());
            glEnd();

            glColor3f(1,0,0);
            glBegin(GL_LINES);
            glVertex2d(GetPosition().GetX(), GetPosition().GetY());
            glVertex2d(arrow.GetX(), arrow.GetY());
            glEnd();

            glColor3f(0,0.5,0);
            stringstream anglestr;
            anglestr << GetSpeed().GetAngle().GetAngle(Angle::eAngleDegreePos) << " degrees";

            GlutProject::DrawText(arrow.GetX(), arrow.GetY(), anglestr.str());

            glPopMatrix();
        }

        void SetLeftWheelSpeed(double value)
        {
            m_LeftWheelSpeed = value;
        }

        void SetRightWheelSpeed(double value)
        {
            m_RightWheelSpeed = value;
        }

        void CalculateWheelSpeed()
        {
            Speed newspeed(0.000001 + Math::Abs(10000 * m_LeftWheelSpeed * m_RightWheelSpeed), Angle(-m_LeftWheelSpeed + m_RightWheelSpeed, Angle::eAngleRad) + GetSpeed().GetAngle());

//            Point newposition(-100000 * m_LeftWheelSpeed * m_RightWheelSpeed, newspeed.GetAngle());

            SetNewSpeed(newspeed);
//            SetNewPosition(GetPosition() + newposition);
        }

        void Reset()
        {
            // Reset Speed //
            m_BreakValue = 0;
        }

        double GetBreak() const
        {
            return m_BreakValue;
        }

        void SetBreak(double value)
        {
            m_BreakValue = value;
        }

    private:
        double m_LeftWheelSpeed;
        double m_RightWheelSpeed;
        double m_BreakValue;
};

